#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

/*
 * Copyright (C) 2022 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http:// www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_errno.h"
#include "common.h"
#include "app_uart.h"
#include "GPS_uart.h"
#include "motor.h"
#include "mpuiic.h"
#include "PScontrol.h"
#include "servor_control.h"
#include "car_task.h"

float pitch,roll,yaw; 								  			 //欧拉角(姿态角)
short aacx,aacy,aacz;													 //加速度传感器原始数据
short gyrox,gyroy,gyroz;											 //陀螺仪原始数据
float temp; 								  								 //温度

void *CarTask(void* param)
{
    printf("prepare for work\r\n");
    unsigned int time = 20;
    MPU_Init();					    			 //=====初始化MPU6050
	mpu_dmp_init();								 //=====初始化MPU6050的DMP模式	

    while (1) {
         mpu_dmp_get_data(&pitch,&roll,&yaw);			//得到姿态角即欧拉角
		temp=MPU_Get_Temperature();								//得到温度值
		MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//得到加速度传感器数据
		MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//得到陀螺仪数  	
	    printf("pitch=%.2f,roll=%.2f,yaw=%.2f,temp=%.2f\n",pitch,roll,yaw,temp/100);
        while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //防止FIFIO溢出
        if(recivestate= true)  
		{ 
            scan_ps2();//control
		}
       osDelay(10);
        }
        IoTWatchDogDisable();
        osDelay(time);
}

void car_task(void)
{ 
    Uart_config();
    osThreadAttr_t attr;

    attr.name = "CarTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = CAR_TASK_SIZE;
    attr.priority = CAR_TASK_PRIORITY;

    if (osThreadNew(CarTask, NULL, &attr) == NULL) {
        printf("Falied to create CarTask!\n");
    }
}
APP_FEATURE_INIT(car_task);
